 /*
 * steering.cpp
 *
 *  Created on: 01.01.2012
 *      Author: ignacio
 */

#include "steering.h"
#include "core.h"

#include <iostream>
using std::cout;
using std::endl;
#include <sstream>
using std::stringstream;
#include <cmath>
using std::sqrt;
using std::sin;
using std::cos;
#include <algorithm>
using std::min;

namespace neurignacio
{

    void Seek::getSteering(SteeringOutput& output)
    {
    	stringstream out;
    	out << "\t~~~~~~~~~~ SEEK - Line " << __LINE__ << endl;
    	out << "\tCharacter Position: " << character->position << " Target: " << *targetPosition;
    	// First work out the direction
        output.linear = *targetPosition;
        output.linear -= character->position;

        // If there is no direction, do nothing
        if (output.linear.squareMagnitude() > 0)
        {
        	output.linear.normalise();
            output.linear *= character->maxAcceleration;
        }
        out << " Output.linear: " << output.linear << endl;
        cout << out.str();
    }

    void Flee::getSteering(SteeringOutput& output)
    {
    	stringstream out;
    	out << "\t^^^^^^^^^^^^^^^ FLEE - Line " << __LINE__ << endl;
    	out << "\tcharacter->velocity: " << character->velocity;
    	// Calculate Steering from Seek
    	Seek::getSteering(output);

    	out << " Seek::output: " << output.linear;

    	if (character->velocity.isZero() && output.linear.isZero()) // character and target are at the precise same point but I'm not moving, therefore I want to flee
    	{
    		output.linear.random(); // Generates a random vector
    		output.linear.y=0; // I'm in 2D :-)
    		output.linear.normalise();
    		output.linear *= character->maxAcceleration;
    		out << " RANDOM OUTPUT: " << output.linear << endl;
    	}
    	// Reverse Steering
    	output.linear *= -1;
    	out << " inverted: " << output.linear;
//		cout<<out.str();
    }

	void Arrive::getSteering(SteeringOutput& output)
	{
    	stringstream out;
		out << "\t*********** ARRIVE: Line " << __LINE__<< "************" << endl;
		out << "\tCharacter Position: " << character->position << " Target Position: " << *targetPosition << endl;
		// First work out the direction
        output.linear = *targetPosition;
        output.linear -= character->position;

		out << "\ttarget->position-character->position=" << output.linear;
        // Calculate distance to target
        real distance = output.linear.squareMagnitude();

        out << " distance: " << output.linear.length() << " slowRadius: " << slowRadius << " targetRadius: " << targetRadius;

        real targetSpeed;
        if (distance > slowRadius*slowRadius) // If we are outside the slowRadius, then go max speed
        {
        	targetSpeed=character->maxSpeed;
        }
        else if (distance<=slowRadius*slowRadius && distance>targetRadius*targetRadius) // If we are inside the slowRadius, calculate a scaled speed
        {
        	targetSpeed= character->maxSpeed*distance / slowRadius;
        }
        else
        {
        	out << " I DO NOTHING!" << endl;
        	return; // Otherwise we are there, then do nothing
        }

        out << " targetSpeed: " << targetSpeed;
        // The target velocity combines speed and direction
        output.linear.normalize();
        output.linear *= targetSpeed;

        // Acceleration tries to get to the target velocity
        output.linear -= character->velocity;
        output.linear /= timeToTarget;

        // Check if the acceleration is too fast
        if (output.linear.squareMagnitude() > character->maxAcceleration*character->maxAcceleration )
        {
        	output.linear.normalize();
        	output.linear *= character->maxAcceleration;
        }
        out << " final output.linear: " << output.linear << endl;
//        cout << out.str();
	}

	void Align::getSteering(SteeringOutput& output)
	{
		stringstream out;
		out << "\t*********** ALIGN: Line " << __LINE__<< "************" << endl;
		out << "\tCharacter Orientation: " << character->orientation << " Target Orientation: " << *targetOrientation;
		// Get the naive direction to the target
		output.angular = *targetOrientation;
		output.angular -= character->orientation;

		out << " target->orientation-character->orientation=" << output.angular;
		// Map the result to the (-pi,pi) interval
		output.angular.normalize_180();
		out << "\n\tNormalize_180: " << output.angular;
		const bool IsNegativeRotation=(output.angular<0);
		angle rotationSize=neurignacio::abs(output.angular);
		out << " Rotation Size: " << rotationSize;
		out << " character->maxRotation: " << character->maxRotation;
		out << " character->maxAngularAcceleration: " << character->maxAngularAcceleration;
		out << "\n\trotation Size: " << (IsNegativeRotation? "Negative! ":"Positive! ");
		out << "Character.rotation: " << character->rotation;


		angle targetRotation;
		if (rotationSize>slowAngularDistance)	// If we are outside the slowRadius, then use maximum rotation
			targetRotation=character->maxRotation;
		else if (rotationSize<=slowAngularDistance && rotationSize>targetAngularDistance)
		{
			targetRotation = character->maxRotation * rotationSize / slowAngularDistance; // if we are inside the slowRadius, calculate a scaled rotation
		}
		else
		{
			//output.angular=0;
			return; // Otherwise we are there, then do nothing
		}

		if (IsNegativeRotation) targetRotation *= -1; // Reverse Rotation is it's negative



		// Acceleration tries to get to the target velocity
		output.angular = targetRotation - character->rotation;
		output.angular /= timeToTarget;

		// Check if the acceleration is too great
		angle angularAcceleration=neurignacio::abs(output.angular);
		if (angularAcceleration>character->maxAngularAcceleration)
		{
			output.angular /= angularAcceleration;
			output.angular *= character->maxAngularAcceleration;
		}
		out << " output.angular=" << output.angular;
		out << endl;

//		cout << out.str();
	}

	void Face::getSteering(SteeringOutput& output)
	{
		stringstream out;
		out << "\t~~~~~~~~~~~~FACE - Line " << __LINE__ << endl;
		out << "\tcharacter->position: " << character->position;
		out << "\ttargetPosition: " << *targetPosition;
		real prediction;
		// 1.	Calculate the target to delegate to Align
		Vector3 direction = *targetPosition;
		direction -= character->position;

		// Check for a zero direction and make no change if so
		if (direction.squareMagnitude()>0)
		{
			out << "\tDirection: " << direction <<  " Direction AngleXZ: " << direction.angleXZ() << endl;
			targetOrientation=&direction.angleXZ();
			Align::getSteering(output);
		}
//		cout << out.str();
	}

	void LookWhereYoureGoing::getSteering(SteeringOutput& output)
	{
		stringstream out;
		out << "\t==================LOOK - Line " << __LINE__ << endl;
		// 1.	Calculate the target to delegate to Align
		// Check for a zero direction and make no change if so
		if (character->velocity.squareMagnitude()>0)
		{
			out << "\tVelocity: " << character->velocity <<  " Direction AngleXZ: " << character->velocity.angleXZ() << endl;
			targetOrientation=&(character->velocity.angleXZ());
			Align::getSteering(output);
		}
//		cout << out.str();

	}

    void Pursue::getSteering(SteeringOutput& output)
    {
    	real prediction;
    	// 1.	Calculate the target to delegate to Seek
    	output.linear = *virtualTargetPosition;
    	output.linear -= character->position;

    	// Check if character velocity is too small to give a reasonable prediction time
    	if (maxPrediction*maxPrediction*character->velocity.squareMagnitude() <= output.linear.squareMagnitude())
    		prediction = maxPrediction;
    	else
    		prediction = output.linear.magnitude() / character->velocity.magnitude();

    	// Put the target together

    	targetPosition = &(*virtualTargetPosition + *targetVelocity * prediction);
    	Seek::getSteering(output);	// CAN I USE ARRIVE INSTEAD?
    }

    void Evade::getSteering(SteeringOutput& output)
	{
    	real prediction;
    	// 1.	Calculate the target to delegate to Seek
    	output.linear = *virtualTargetPosition;
    	output.linear -= character->position;

    	// Check if character velocity is too small to give a reasonable prediction time
    	if (maxPrediction*maxPrediction*character->velocity.squareMagnitude() <= output.linear.squareMagnitude())
    		prediction = maxPrediction;
    	else
    		prediction = output.linear.magnitude() / character->velocity.magnitude();

    	// Put the target together

    	targetPosition = &(*virtualTargetPosition + *targetVelocity * prediction);
    	Flee::getSteering(output);
	}

	void VelocityMatch::getSteering(SteeringOutput& output)
	{
		// First work out the direction
		output.linear = *targetVelocity;
		output.linear -= character->velocity;

		output.linear /= timeToTarget;

		// Check if the acceleration is too fast
		if (output.linear.squareMagnitude() > character->maxAcceleration*character->maxAcceleration )
		{
			output.linear.normalize();
			output.linear *= character->maxAcceleration;
		}
	}

	void Wander::getSteering(SteeringOutput& output)
	{
		stringstream out;
		out << "\t::::::::::::::::: WANDER - Line "<< __LINE__ << endl;
		out << "\tcharacter->orientation: " << character->orientation;
		out << " character->orientation.asVector()" << character->orientation.asVector();
		out << " character->orientation.asVector.length(): " << character->orientation.asVector().length();
		out << endl;
		angle dummyOrientation = character->orientation + wanderRate*randomBinomial();
		targetOrientation=&dummyOrientation;
//		targetOrientation = &(character->orientation + randomBinomial()*wanderRate);
		out << "\ttargetOrientation: " << *targetOrientation;
		Vector3 dummyPosition = character->position + character->orientation.asVector()*wanderOffset;
		dummyPosition += targetOrientation->asVector()*wanderRadius;
		targetPosition = &(dummyPosition);
		out << " targetPosition: " << *targetPosition << endl;
		Face::getSteering(output);
		output.linear = character->orientation.asVector() * character->maxAcceleration;
		out << "\toutput.linear: " << output.linear << " output.angular: " << output.angular << endl;
//		cout << out.str();
	}

	void Separation::getSteering(SteeringOutput& output)
	{
		stringstream out;
		out <<  "\tooooooooo SEPARATION - Line " << __LINE__ << endl;

		output.linear = character->position;
		output.linear -= *targetPosition;


		real distance = output.linear.length();
		out <<  "\ttargetPosition: " << *targetPosition << " character->position: " << character->position << " difference: " << output.linear << " distance: " << distance << " threshold: " << thresholdDistance << endl;
		real strength;
		if ( distance < thresholdDistance)
			strength = std::min(decayCoefficient / (distance*distance),character->maxAcceleration);
		else
			strength = 0;
		output.linear.normalise();
		output.linear *= strength;
		out <<  "\tstrength: " << strength;
		out <<  " output.linear: " << output.linear << endl;
		//		cout << out.str();
	}

	void Attraction::getSteering(SteeringOutput& output)
	{
		Separation::getSteering(output);
		output.linear *= -1;
	}

	void CollisionAvoidance::getSteering(SteeringOutput& output)
	{
		stringstream out;
		out <<  "\txxxxxxxxx COLLISION - Line " << __LINE__ << endl;
		// Calculate the time to collision
		Vector3 relativePosition = *targetPosition - character->position;
		Vector3 relativeVelocity = *targetVelocity - character->velocity;
		real relativeSpeed = relativeVelocity.length();
		real distance = relativePosition.length();
		real timeToCollision=0;
		if (distance<=collisionRadius) 	// Somehow we already collided!
			timeToCollision=0;
		else
		{
			if (!relativeSpeed)		// we need to about division by zero!
				relativeSpeed = character->minVelocity;
			timeToCollision = -1*Vector3::dot(relativePosition, relativeVelocity) / (relativeSpeed * relativeSpeed);
		}
		out <<  "\tcharacter->position: " << character->position << " targetPosition: " << *targetPosition << " relativePosition: " << relativePosition << endl;
		out <<  "\tcharacter->velocity: " << character->velocity << " targetVelocity: " << *targetVelocity << " relativeVelocity: " << relativeVelocity << endl;
		out <<  "\trelativeSpeed: " << relativeSpeed << " relativeDistance="<< distance<<" timeToCollision: " << timeToCollision << " maxPrediction: " << maxPrediction << endl;
		out <<  "\tPchar=" << character->position+character->velocity*timeToCollision << " Ptar=" << *targetPosition+*targetVelocity*timeToCollision << endl;

		// Check if it is going to be a collision at all
		// within a reasonable prediction time
		real minSeparation = distance-relativeSpeed*timeToCollision;
		out <<  "\tdistance: " << distance << " minSeparation: " << minSeparation << " collisionRadius: " << collisionRadius << endl;
		if (minSeparation > collisionRadius || timeToCollision <0 || timeToCollision>maxPrediction)
		{
			output.linear.clear();
			out <<  "\toutput.linear: " << output.linear << endl;
			return; // continue
		}

		// Check if it is the shortest
		// THIS NEEDS TO BE IMPLEMENTED IN THE CASE OF MULTIPLE TARGETS

		// Calculate the steering

		// If we're going to hit exactly, or if we're already colliding,
		// the steering is based on current position (already calculated relativePosition)
		// otherwise we predict relativePosition based on timeToCollision
		if (minSeparation >0 && distance > collisionRadius)
			relativePosition = relativePosition + relativeVelocity * timeToCollision;
		out <<  "\trelativePosition: " << relativePosition;
		relativePosition *= -1;
		// Avoid the target
		relativePosition.normalize();
		output.linear = relativePosition * character->maxAcceleration;
		out <<  " output.linear: " << output.linear << endl;
		//		cout << out.str();
	}

	void SeekPath::getSteering(SteeringOutput& output)
	{
		// 1. Calculate the target to delegate
		//    Find the predicted future location
		stringstream out;
		Vector3 futurePosition = character->position + character->velocity * maxPrediction;
		out << "Future Position: " <<  futurePosition;
		//    Find the current position in the path
		real currentParameter = targetPath->getParameterClosestToPoint(futurePosition);
		out << " currentParementer: " << currentParameter;
		//    Offset it
		real targetParameter = currentParameter + pathOffset;
		out << " targetParameter: " << targetParameter;
		// Get the target position
		targetPosition = &(targetPath->getPointFromParameter(targetParameter));
		out << " targetPosition: " << *targetPosition;
		out << endl;
		cout << out.str();
		// 2. Delegate to Seek
		Seek::getSteering(output);
	}






}; // end of namespace neurignacio


